package utils;

import java.lang.Math;

public class Vector3D {

	public double [] v = new double[3];

	public Vector3D() {
		v[0] = v[1] = v[2] = 0;
	}

	public Vector3D( double x, double y, double z ) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
	}

	public Vector3D( Point3D P ) {
		v[0] = P.p[0];
		v[1] = P.p[1];
		v[2] = P.p[2];
	}
	
	public Vector3D( Point3D P, Point3D P2 ) {
		v[0] = P2.p[0] - P.p[0];
		v[1] = P2.p[1] - P.p[1];
		v[2] = P2.p[2] - P.p[2]; 
	}

	public Vector3D(double x, double y) {
		v[0] = x;
		v[1] = y;
		v[2] = 0;
	}

	public void copy( double x, double y, double z ) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
	}

	public void copy( Vector3D V ) {
		v[0] = V.v[0];
		v[1] = V.v[1];
		v[2] = V.v[2];
	}

	public double x() { return v[0]; }
	public double y() { return v[1]; }
	public double z() { return v[2]; }

	public double lengthSquared() {
		return x()*x() + y()*y() + z()*z();
	}
	public double length() {
		return (float)Math.sqrt( lengthSquared() );
	}

	public Vector3D negated() {
		return new Vector3D(-x(),-y(),-z());
	}

	public Vector3D normalized() {
		double l = length();
		if ( l > 0 ) {
			double k = 1/l; // scale factor
			return new Vector3D(k*x(),k*y(),k*z());
		}
		else return new Vector3D(x(),y(),z());
	}

	// returns the dot-product of the given vectors
	static public double dot( Vector3D a, Vector3D b ) {
		return a.x()*b.x() + a.y()*b.y() + a.z()*b.z();
	}

	// returns the cross-product of the given vectors
	static public Vector3D cross( Vector3D a, Vector3D b ) {
		return new Vector3D(
			a.y()*b.z() - a.z()*b.y(),
			a.z()*b.x() - a.x()*b.z(),
			a.x()*b.y() - a.y()*b.x()
		);
	}

	// returns the sum of the given vectors
	static public Vector3D sum( Vector3D a, Vector3D b ) {
		return new Vector3D( a.x()+b.x(), a.y()+b.y(), a.z()+b.z() );
	}

	// returns the difference of the given vectors
	static public Vector3D diff( Vector3D a, Vector3D b ) {
		return new Vector3D( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() );
	}

	// returns the product of the given vector and scalar
	static public Vector3D mult( Vector3D a, double b ) {
		return new Vector3D( a.x()*b, a.y()*b, a.z()*b );
	}

	// Returns the angle, in [-pi,pi], between the two given vectors,
	// and whose sign corresponds to the right-handed rotation around
	// the given axis to get from v1 to v2.
	static public double computeSignedAngle( Vector3D v1, Vector3D v2, Vector3D axisOfRotation ) {

		Vector3D crossProduct = Vector3D.cross( v1.normalized(), v2.normalized() );

		// Due to numerical inaccuracies, the length of the cross product
		// may be slightly more than 1.
		// Calling arcsin on such a value would result in NaN.
		double lengthOfCross = crossProduct.length();
		double angle = ( lengthOfCross >= 1 ) ? (double)Math.PI/2 : (double)Math.asin( lengthOfCross );

		if ( Vector3D.dot( v1, v2 ) < 0 )
			angle = (float)Math.PI - angle;
		if ( Vector3D.dot( crossProduct, axisOfRotation ) < 0 ) angle = -angle;
		return angle;
	}
	
	static public Vector3D getBissectrice(Vector3D a, Vector3D b)
	{
		a = a.normalized();
		b = b.normalized();
		Vector3D c;
		if(a.equals(b) || a.equals(b.negated()))
		{
			c = new Vector3D(- a.y(), a.x(), a.z());		
		}
		else
		{
			c = Vector3D.sum(a.negated(),b);
		}
		
		return c.normalized();
	}
	
	@Override 
	public boolean equals(Object o)
	{
		Vector3D v = (Vector3D)o;
		if(this.x() == v.x() && this.y() == v.y() && this.z() == v.z() )
		{
			return true;
		}
		return false;
	}
	
	public String toString()
	{
		return "(" +v[0]+","+v[1]+","+v[2]+")";
	}

	public static double computeSignedAngle(Vector3D v1, Vector3D v2) {
		//return Vector3D.computeSignedAngle(
		//	new Vector3D( v1.x(), v1.y(), 0 ),
		//	new Vector3D( v2.x(), v2.y(), 0 ),
		//	new Vector3D( 0, 0, 1 )
		//);

		double productOfLengths = v1.length() * v2.length();
		if ( productOfLengths <= 0 )
			return 0;

		// Compute the z component of the cross product of v1 and v2
		// (Note that the x and y components of the cross product are zero,
		// because the z components of a and b are both zero)
		double crossProduct_z = v1.x()*v2.y() - v1.y()*v2.x();

		double sineOfAngle = Math.abs(crossProduct_z) / productOfLengths;

		// Due to numerical inaccuracies, the sine we computed
		// may be slightly more than 1.
		// Calling arcsin on such a value could be bad, so we don't.
		double angle = ( sineOfAngle >= 1 ) ? Math.PI/2 : Math.asin( sineOfAngle );

		// Compute the dot product of v1 and v2
		double dotProduct = Vector3D.dot( v1, v2 );

		if ( dotProduct < 0 )
			angle = Math.PI - angle;

		if ( crossProduct_z < 0 )
			angle = - angle;

		return (double)angle;
	}

	public double angle() {
		return angle( 0 );
	}
	
	public double angle(double lowerBound) {
		double l = length();
		if ( l <= 0 )
			return lowerBound;
		double angle = (double)Math.asin(y()/l);
		// Now angle is in [-pi/2,pi/2]
		if ( x() < 0 )
			angle = (double)Math.PI - angle;
		// Now angle is in [-pi/2,3*pi/2]

		double TwoPi = (double)( 2*Math.PI );
		double upperBound = lowerBound + TwoPi;
		while ( angle > upperBound )
			angle -= TwoPi;
		while ( angle < lowerBound )
			angle += TwoPi;
		// Now angle is in [lowerBound, upperBound]
		return angle;
	}

    public static Vector3D Truncate(Vector3D vec, float max_value)
    {
        if (vec.length() > max_value)
        {
        	Vector3D normalizedVec = new Vector3D();
        	normalizedVec.copy(vec);
        	normalizedVec = normalizedVec.normalized();
            return Vector3D.mult(normalizedVec, max_value);
        }
        return vec;
    }

}
